#include <iostream>
#include <string.h>

#include <pthread.h>
#include <stdlib.h>
#include <stdio.h>
#include <semaphore.h>

#include <sys/time.h>
#include <unistd.h>
#include <sys/select.h>


#include "constants.h"
#include "other_functions.h"

#include "drawmodel.h"
#include "view.h"
#include "model.h"

#include "SMATModel.hh"
#include "display.hh"
#include "utils.hh"

/* Sort points in top-to-bottom order */
int CDrawModel::compareZ_line_t( const void* _a, const void* _b)
{
    struct line_t *a = ((struct line_t*)_a);
    struct line_t *b = ((struct line_t*)_b);
    
    if (a->z2 < a->z1)
    {
	if (b->z1 < b->z2)
	    return a->z2 < b->z1;
	else 
	    return a->z2 < b->z2;
    }
    else
    {
	if (b->z1 < b->z2)
	    return a->z1 < b->z1;
	else 
	    return a->z1 < b->z2;
    }

}

void CDrawModel::DrawModel(CView2D &view, 
			   IplImage * image, CvMat *mask_color, 
			   int index_point,
			   CvScalar color)
{
    DrawModel(view, view.points2d, view.mask_show, 
	      view.p_linkmodel->lines, 
	      image, mask_color, index_point, color);
}

void CDrawModel::DrawModel3D(CView2D &view, 
			     IplImage * image, CvMat *mask_color, 
			     int index_point,
			     CvScalar color)
{
    view.reproject2();
    DrawModel(view, view.points2d_reproject, view.mask_show, 
	      view.p_linkmodel->lines, 
	      image, mask_color, index_point, color);
}

void CDrawModel::DrawModel3D(CModel3D &model, CView &view, 
			     IplImage * image, CvMat *mask_color, 
			     int index_point,
			     CvScalar color)
{
    cvProjectPoints2(model.points3d, view.R, view.T, view.calibration->KK, NULL, model.points2d);
    DrawModel(view, model.points2d, view.mask_show, 
	      model.lines, 
	      image, mask_color, index_point, color);
}


void CDrawModel::DrawModel(CView &view, CvMat * points, CvMat *mask_show,
			   CvMat * lineas, 
			   IplImage * image, CvMat *mask_color, 
			   int index_point,
			   CvScalar color)
{
    int cblue, cgreen, cred;
    int j, i, index;
    int  x, y, x2, y2;
    char buffer[300];

    CvSize size = cvGetSize(lineas);
    int npoints = size.height;
    int n = size.width;

    for (i=0; i<npoints; i++)
    {
	if (!cvGetReal1D(mask_show,i))
	    continue;
	x = (int)cvmGet(points, i, 0);
	y = (int)cvmGet(points, i, 1);
	
	if (y<2) continue;
	if (y>=image->height-2) continue;
	if (x<2)  continue;
	if (x>=image->width-2)  continue;

	cblue = 0;
	cgreen = 0;
	cred = 0;

	if (i==index_point)
	    cred = 255;
	else if (mask_color != NULL && mask_color->data.ptr[i])
	    cblue = 255;
	else
	    cgreen = 185;
		
	sprintf(buffer, "%d", i);
	cvPutText(image, buffer, cvPoint(x+1, y-1), &fontmod, CV_RGB(cred,cgreen,cblue));

	//dibuja las líneas a los puntos más cercanos
	for (j=0; j<n; j++)
	{
	    index = (int)cvmGet(lineas, i, j);
	    if (!cvGetReal1D(mask_show,index))
		continue;
	    x2 = (int)cvmGet(points, index, 0);
	    y2 = (int)cvmGet(points, index, 1);
	    cvLine(image, cvPoint(x, y), cvPoint(x2, y2), color, 2, 8 );   
	}
    }


    //draw the axis
    struct line_t plineas[] = {
	 {0, 1, 0, 0, CV_RGB(255,60,60), 4}
	,{0, 2, 0, 0, CV_RGB(30,255,30), 4} 
	,{0, 3, 0, 0, CV_RGB(120,120,255), 4}
	//,{0, 1, 0, 0, CV_RGB(155,0,0), 1}
	//,{0, 2, 0, 0, CV_RGB(0,105,0), 1} 
	//,{0, 3, 0, 0, CV_RGB(0,0,245), 1}

	,{3, 4, 0, 0, CV_RGB(220,220,228), 3}
	,{3, 5, 0, 0, CV_RGB(220,220,228), 3}
	,{3, 6, 0, 0, CV_RGB(220,220,228), 3}
	,{3, 7, 0, 0, CV_RGB(220,220,228), 3}
	//,{3, 4, 0, 0, CV_RGB(80,80,88), 1}
	//,{3, 5, 0, 0, CV_RGB(80,80,88), 1}
	//,{3, 6, 0, 0, CV_RGB(80,80,88), 1}
	//,{3, 7, 0, 0, CV_RGB(80,80,88), 1}
    };

    int NLINEAS_EJES = sizeof(plineas)/sizeof(struct line_t);
    CvPoint p1, p2;

    cvmSet(translation2, 2, 0, view.Tz());
    cvProjectPoints2(ejes3D, view.R, translation2, view.calibration->KK, NULL, ejes2D);

    //rota la matrizde ejes para ver el orden de dibujar los ejes
    
    cvRodrigues2(view.R, rotation);
    cvGEMM (ejes3D, &rodriguesZ, 1.0F, NULL, 0, ejes3DZ, CV_GEMM_B_T);
    // paso los valores de Z rotado a la matriz de lineas
    for(i=0; i<NLINEAS_EJES; i++)
    {
	plineas[i].z1 = cvmGet(ejes3DZ, plineas[i].p1, 0);
	plineas[i].z2 = cvmGet(ejes3DZ, plineas[i].p2, 0);
    }

    //ahora hay que ordenar por valor de coordenada Z
    qsort((void*)plineas, NLINEAS_EJES, sizeof(struct line_t), compareZ_line_t);

    int offsetx = cvmGet(ejes2D, plineas[0].p1, 0) - OX;
    int offsety = cvmGet(ejes2D, plineas[0].p1, 1) - OY;

    for (i=0; i<NLINEAS_EJES; i++)
    {
	p1.x = (int)cvmGet(ejes2D, plineas[i].p1, 0) - offsetx;
	p1.y = (int)cvmGet(ejes2D, plineas[i].p1, 1) - offsety;
	p2.x = (int)cvmGet(ejes2D, plineas[i].p2, 0) - offsetx;
	p2.y = (int)cvmGet(ejes2D, plineas[i].p2, 1) - offsety;

	cvLine(image, p1, p2, plineas[i].color, plineas[i].thick, 4);
    }	    

    sprintf(buffer, "+X");
    cvPutText(image, buffer, cvPoint((int)cvmGet(ejes2D,1,0)-offsetx+10, 
				     (int)cvmGet(ejes2D,1,1)-offsety+10), &fontmod2, CV_RGB(0,0,0));
    sprintf(buffer, "+Y");
    cvPutText(image, buffer, cvPoint((int)cvmGet(ejes2D,2,0)-offsetx+10, 
				     (int)cvmGet(ejes2D,2,1)-offsety+10), &fontmod2, CV_RGB(0,0,0));

    int TEXTPOSX = 25;
    int TEXTPOSY = 380;
    
    CvScalar col = CV_RGB(125,255,0);
    sprintf(buffer, "T=(%5.1f, %5.1f, %5.1f)", view.Tx(), view.Ty(), view.Tz());
    cvPutText(image, buffer, cvPoint(TEXTPOSX, TEXTPOSY+15), &fontmod, col);
    sprintf(buffer, "a=%7.3f", view.Rx()*180.0F/CV_PI);
    cvPutText(image, buffer, cvPoint(TEXTPOSX, TEXTPOSY+30), &fontmod, col); 
    sprintf(buffer, "b=%7.3f", view.Ry()*180.0F/CV_PI);
    cvPutText(image, buffer, cvPoint(TEXTPOSX, TEXTPOSY+45), &fontmod, col); 
    sprintf(buffer, "g=%7.3f", view.Rz()*180.0F/CV_PI);
    cvPutText(image, buffer, cvPoint(TEXTPOSX, TEXTPOSY+60), &fontmod, col); 
}

